#include "common/se2.h"

using namespace std;

SE2::SE2() {

}

SE2::SE2(const double& x, const double& y, const double& t) : so2_(SO2(t)), p_(Eigen::Vector2d(x, y)) {

}

SE2::SE2(const SO2& so2, const Eigen::Vector2d& p) : so2_(so2), p_(p) {

}

SE2 SE2::Inv() {
    double c = std::cos(so2_.log());
    double s = std::sin(so2_.log());
    double x = p_[0];
    double y = p_[1];
    Eigen::Vector2d p_inv = Eigen::Vector2d(-c*x-s*y, s*x-c*y);
    return SE2(-so2_.log(), p_inv);
}

// Same as: that.inv() + *this
SE2 SE2::operator +(const SE2& that) const{
    double theta = SO2(so2_).log();
    double that_theta = SO2(that.so2_).log();

    double c = std::cos(theta);
    double s = std::sin(theta);
    double x = p_[0] + that.p_[0]*c - that.p_[1]*s;
    double y = p_[1] + that.p_[0]*s + that.p_[1]*c;
    SO2 t = SO2(theta + that_theta);
    return SE2(t, Eigen::Vector2d(x, y));
}

SE2 SE2::operator -(const SE2& that) const{
    Eigen::Vector2d dp = p_-that.p_;
    double dx = dp[0];
    double dy = dp[1];

    SO2 dt = so2_- SO2(that.so2_);
    double dth = dt.log();

    float c = std::cos(SO2(that.so2_).log());
    float s = std::sin(SO2(that.so2_).log());
    return SE2(c*dx+s*dy, -s*dx+c*dy, dth);
}

Eigen::Matrix3d SE2::matrix() {
    Eigen::Matrix3d p = Eigen::Matrix3d::Identity();
    p.block<2,2>(0,0) = so2_.matrix();
    p.block<2,1>(0,2) = p_;
    return p;
}

Eigen::Matrix4d SE2::matrix4d(const SO2& so2, const Eigen::Vector2d& p) {
    Eigen::Matrix4d pose = Eigen::Matrix4d::Identity();
    pose.block<2,2>(0,0) = SO2(so2).matrix();
    pose.block<2,1>(0,3) = p;
    return pose;
}

Eigen::Matrix3d SE2::skew(const Eigen::Vector3d& p) {
    Eigen::Matrix3d skew_p;
    skew_p << 0, -p.z(), p.x(),
              p.z(), 0, p.y(),
              -p.x(), -p.y(), 0;
    return skew_p;
}